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ieee ODUCTION 


A. GENERAL 

The crux of this thesis is to develop, test and evaluate 
a real-time state-space derived dive plane controller for 
the NPS prototype Autonomous Underwater Vehicle (AUV). The 
U.S. Navy and a number of its contractors are presently 
developing unmanned miniature submarines (AUV's) for several 
important underwater missions. These include surveillance, 
submarine tracking, submarine decoys, bottom surveying, and 
off-board . sensor support for both surface ships’ and 
submarines [Ref. 1]. The shape of these vehicles is similar 
to that of a large torpedo and can be deployed on 
submarines, and surface ships. 

The AUV testbed at the Naval Postgraduate School (NPS) 
currently being developed is to examine the use of, and 
problems associated with, advanced control technologies. 
The objectives include: 


- determine requirements imposed by the implied need for 
artificial intelligence, 


-~ development of advanced computer control concepts and 
architecture, 


- determine system interface feasibility. [Ref. 2] 
Unlike the conventional body-of-revolution design, the 
NPS testbed iS similar to a low aspect ratio wing. It is 


Similar conceptually to the late model Swimmer Delivery 


Vehicle (SDV) design, but actually based on a two and three 
to one aspect ratio box shape vehicle. Vehicle selection 
was based initially on the availability and thoroughness to 
which the vehicle hydrodynamic characteristics were modeled 
and validated, providing for ease of program development and 
verificatzoneiRetawede 

Modern trends in control system design are moving 
towards greater complexity. The desire for complex tasks 
and greater accuracy fuel the need for high powered 
computers to accomplish real-time control tasks in short 
durataion. One of the main interests here is to determine 
the capabilities of real-time control with adaptive 
autopilots and various computer architectures. The need for 
adaptivity lies in the need to achieve robust performance 
over a wide range of operating conditions. Also, although 
computational speed may suffer, rapid response is seen -to be 
important when evaSive and/or obstacle avoidance maneuvers 
are called for at the supervisory/expert system level. It 
follows that the design trade-off between response and 
robustness requires examination. For these reasons, this 
particular work has concentrated on the development of a 
real-time controller including the ability to perform 
parameter estimation, and the issues surrounding its 


response time performance. 


B. REVIEW OF PREVIOUS WORK 

The initial proposal from the Naval Postgraduate School 
for research on an Autonomous Underwater Vehicle began 1 
October 1987. It was titled "Navigation, Path Planning, 
Dynamics and Control of Generic Autonomous Underwater 
Vehicles," and was submitted to the Naval Surface Weapons 
Center in Silver Spring, Maryland. The primary stated 
objective was to assess the capabilities for the design, 
construction, testing, and operation of a testbed as a 
research platform for demonstrating the performance of AUVs 
met. 2). 

The first task involved selecting a suitable submersible 
model. A non-conventional wing-shaped model with body cross 
sections nearly rectangular rather than circular became the 
selected choice. 

Using Dynamic Simulation Language (DSL), Boncal (1987) 
modified the existing equations of motion (DTNSRDC 2510) and 
performed all the necessary differentials for their lineari- 
zation to suit the needs of the AUV controller development. 
The linearization was necessary to the design of model-based 
controls and to simulate controlled responses for all six 
degrees of freedom of vehicle motion [Ref. 3}. Both linear 
and non-linear models were developed and evaluated. This 
lead to the initial development of a controller robust 
enough to handle a variety of reflexive type maneuvers over 


a wide range of speeds (in the programming language DSL). 


In order to investigate the real-time control issues, 
and to provide a means to generate experimental data for the 
evaluation of parameter identification methods, a later 
effort by Brunner (1988), set out to design, build, and test 
a model 30 inches in length and 7 inches wide. The model 
was self-propelled and remotely controlled via radio 
transmission. It was equipped with rate gyro sensors, a 
pressure cell depth sensor and a pressure cell speed sensor. 
Two DC motors provided the propulsion power [Ref. 4]. Open- 
loop dive plane dynamic response tests were performed in a 
water tank where response measurements of depth were made by 
pressure cells, and pitchrate by a pitchrate gyro. Data 
from each run were recorded using PCLAB and the DT NOTEBOOK 
(Ref. 5] data acquisition software, and analyzed using the 
MATRIXx [Ref. 6] data analysis capability. Comparing these 
data with the computer model simulation, hydrodynamic 
coefficients and system transfer functions were developed. 

Initial developments of a digital autopilot to support 
the NPS test vehicle were made by Delaplane (1988), wherein 
an IPM PC/AT was used in conjunction with DATA TRANSLATION 
Analog to Digital (A/D) interfacing boards, as the platform 
for the real-time closed loop controller. A simplified 
control program for AUV dive plane commands had _ been 
implemented. Signal generators were used to replicate AUV 


transducer outputs of depth, speed, and pitchrate to verify 


system operation [Ref. 7}. The inclusion of real control 
laws, and the system parameter identification, however, was 


left to the activity reported herein. 


Cc. SCOPE OF WORK 

The scope of this investigation was to develop the 
digital autopilot [Ref. 7] into a three state feedback, 
Single Input multiple Output, real-time closed-loop dive 
control system for the NPS AUV model. Initial stages of 
research lead to the notion that the pitch-heave coupling 
prevalent in many underwater vehicles was not significant 
for this vehicle. The three state UiMieb les of the dynamic 
model were then reduced to the pitch rate, pitch angle, and 
the water depth. Because of tank length limitations it was 
foreseen that an analog computer model of the vehicle 
dynamics would be essential to the ease of the real time 
control development, and therefore, a scaled simulator was 
Douek . 

The final stages of program development were to 
implement the digital dive plane controller compatible with 
the 30 inch AUV model. Considering the small vehicle size, 
and relatively high component costs, the NPS AUV was unable 
to include a pitch angle sensor. Since pitch angle feedback 
was later shown to be critical to the autopilot stability, 
where the pitch angle could not be measured directly, a 
state reconstruction system was needed and had to be 


designed. This led to the development of a full state 


observer driven by measured inputs and outputs [Ref. 8]. 
The Observer, including the identification of pitch rate 
sensor bias, the bias compensation, and its real time 


performance was large part of this effort. 


D. APPROACH 

Primarily, a solid understanding of the  software/ 
hardware used was essential. In this control system, a high 
level programming language (Turbo Pascal) was interfaced 
with an A/D translation board (hardware) via a software 
package (PCLAB). References 9, 10, 11 established a 
sufficient base on which the elaborate program schemes could 
be developed effectively. Once accomplished, the challenge 
of debugging large programs could readily be resolved. 

Secondly, incorporating experimental data from previous 
AUV research proved paramount. This research thesis could 
not be accomplished without the foundation set by preceding 
studies. 

Lastly, using real-time feedback of AUV model dynamics 
via the analog computer strengthened the ease by which 
program development could proceed. The Analog Computer 
Simulated the model dynamics including the sensor bias and, 
partially, the effects of changing speed. This provided for 


the preliminary design of a robust state-space controller. 


BE. WHAT#POLLOWS 


In Chapter II, some detail is provided concerning the 


models of the vehicle dynamics. Chapter III discusses the 
analog computer modelling. Chapter IV gives an account of 
the program development. Chapters V and VI contain a 


discussion of results and Chapter VII is a summary and 


conclusion. 


II. SIMPLIFIED MODELING" OF VENTCRE* DIVE Be narmme. 


A. GENERAL 

In this chapter, a review of earlier experiments 
conducted under the work of Brunner [Ref. 4], appears and 
the details of the third-order model of the vehicle is 
given. This leads to a discussion of the design issues in 
the controller and the observer systems and the special 
problem of identifying the pitch rate sensor bias from input 


and output data. 


B. REVIEW OF OPEN LOOP EXPERIMENTS 

Open loop experiments were conducted during January, 
1988 (Brunner), in which the model AUV was driven by open 
loop command to perform dive plane maneuvers. From ‘these 
test runs, MATRIXx datafiles were constructed for both the 
command voltage to the dive plane and the measured voltages 
corresponding to the pitch rate, and the water depth 
responses. Using the linear model time history response 
features in MATRIXx, together with many simulation runs in 
which hydrodynamic parameters were varied both in value and 
in combinations of values, several "best fit" parameter 
combinations were synthesized to represent the dynamics of 
the model AUV. It was discovered that the influence of 


coupling between the heave mode, and the pitch mode dynamics 


was not strong in determining the pitch rate behavior, and 
that for the purposes of control shape design, the heave 
motion would be neglected. The influence of the power cord 
was found to be a determining factor in the overall heave 
response, but future tests would be conducted by 
neutralizing the weight of the cord, and this effect would 
not adversely affect closed loop results. 

The numerical values of the vehicle parameters, with 
their respective dimensional units, were established as 


follows in the next section. 


C. THIRD-ORDER MODEL FOR DIVE-PLANE DYNAMICS 

The vehicle equations of motion in the dive plane are 
the surge, heave, and pitch motion equations together with 
the kinematical relationships linking the _ global 
longitudinal and vertical velocity components to the body 
fixed surge, heave, and pitch rates. These are given in 
detail in Boncal (1987). When linearized about a constant 
speed straight line flight path, the relationships simplify 
to four first-order state equations in the heave and pitch 
rates and positions. In short, 

ZW*UXW | ZO*UX*q _ ZDAUX*S | FI 
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where w, q, ©, za are the 
parameters are given by, 
HEAVE DAMPING 
PITCH CROSS COUPLING 
HEAVE CROSS COUPLING 
PITCH DAMPING 
MOMENT EFFECT 
MOMENI: Eh es 
MASS PLUS ADDED MASS 
INERTIA PLUS ADDED MASS 
EFFECT OF TETHER 
EFFECT OF TETHER 


FORWARD SPEED 


LENGTH 


ZW 


ZQ 


MQ 
ZD 
MD 
MZ DW 
et 
ical 
Fe 
UX 


L 


four state variables and the 


O. 225 
1.005 


0.072 


2.1 FEET/SEC 


2.5 FEET, 


where the units of the parameters are given in Table 2.1. 


TABLE 2.1 


PARAMETER UNITS 


| Value 
Parameter Units (Dimensional) 
MQ*UX/L*IYY sec 1 10.0 1/sec 
UX ft/sec 10.0 ft/sec 
MD*UX2/L21YY rad/sec? LORO 1/sec? 


10 


Value 


(Volts Units) 


It should be pointed out that the matching of experimental 
Gata was done by comparison of sensor output voltage 
response time history and that proper conversion to 
dimensional values was essential to being able to translate 
to control gains for real time control system design. 
Because it was found that the effects of the heave/pitch 
coupling were not strong, and that the influence of w was 
dominated by the power cord, it was assumed that for the 
purpose of control system design, the heave dynamics would 
remain unmodelled, and the model base would be reduced to 


third-order with the equations as follows: 


BSH Sot ay 
8° «(Ok Ome «= (Om oe: 53a 

x = iO OMOP On een | O10 . 
0.0 =251 0.0 0.0 


D. FULL STATE FEEDBACK CONTROL DESIGN 
For the nominal forward speed of 2.1 ft/sec, the system 


dynamics matrix, and the system input and output matrices 


are’. 
wills ORO 0.0 Dew 
A = JG, 0.0 0.0 B= 0.0 Co ..0,°0.0,71..0 | 
Dee = 23, I. 0 0.0 


1a 


These equations are characterized by the open loop 


eigenvalues: 


Vv = [-1.8,0.0,0.0] 


which properly indicates the first-order pole for the pitch 
rate response and the double integration from pitch rate to 
depth. The output matrix has diagonal coefficients with 
either unity values or other constants depending on whether 
the output signals are considered to be sensor output 
voltages or the dimensional values of the system state 
variables. 

Noting that the open loop system is completely 
controllable in the formal sense, a pole placement technique 
was determined to be appropriate for the autopilot design. 
Additionally, since the overall system required a digital 
controller, discrete time methods were needed, and the 
question of sample rate arose. 

Digital systems for the control of continuous processes 
are limited to providing a sequence of stepwise constant 
levels that can be updated every sample time, but remain 
fixed within that interval. Clearly, a short interval is 
preferred. However, the execution time of extensive amounts 
of code requires that the sampling period must be sufficient 


for code execution, but it must not be so short as to 


2 


places limitations on the closed loop poles as specified for 
the control design, being consistent with the execution time 
of the real time code. 

In balancing the foregoing, two systems were designed. 
The first, at 20 Hz, was designed to accommodate a control 
code that executed a full three state feedback design where 
the system parameter identification was bypassed. The 
second was at 5 Hz (the maximum upper limit possible under 
the present hardware configuration) where parameter 
identification was performed. 

The open loop system dynamics matrix and input matrix 
were then discretized using the 20 Hz sample rate so that 


the new A, and B matrices became: 


~9 139 0.0 0.0 oueZz i 
A = ~0478 1.0 0.0 B = OOo 
= s0W2 5 = =. LODS0 Ee -.0001 


Closed loop poles for the 20 Hz system were then specified 


at 


B= (Og 2 25 , ©2926 |) 


in the z-domain, corresponding to three poles in the s-plane 
with time constants equal to 1.0 sec, with the following 


state feedback gains, 
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Ke*x = [0.3066,0.8604,-0.2493], 


where allowances were made for the output transducer gain. 


For a state regulator system, the control law is, 
u = -Kc*x 


when constant non-zero command signals are to be introduced, 


the control law requires modification to, 
u = N*¥r —- Kcex*x 
in which the command(s) r and the set point scaling matrix, 
N, are specified so that the tracking error r-y vanishes. 
The closed loop system equations become 
xX = (A-B*Kc) *xX+B4*N*r; v= Cex 
and the tracking error vanishes in steady state when, 


C* (A-B*Kc) ~1xB*N=I; N=(C* (A-B*Kc) ~1%B) 


yielding the discrete time control law used, 


14 


Bee = —0m2493%*/( 1 (kK) -2(kK) }-2(k) J 


-0.8604*[6 (k) ]-0.3066*[g(k) ] 


This control provides reasonable response time and calls for 
a control surface activity level that would exceed the 


stroke of the vehicle dive plane. 


E. OBSERVATION OF UNMEASURED STATES 

In the development of the model AUV the size limitation 
prevented the inclusion of an angle sensor for pitch, so the 
control system was required to reconstruct that signal in 
order to yield stable results. The application of a full 
state observer, configured aS a model based compensator is 


given by the following one step prediction equations, 


we “A “ 


x(k+1) = A*x(k) + B*u(k) + Ko*[y(k) -C*x(k) ] 


in which y(k) is the previous time sensor output signals. 
In this case, two outputs were available to the observer, 
the pitch rate, and the water depth. The observer gains, 
Ko, may be selected by using the dual of the controller pole 
placement technique, or by using Linear Quadratic Regulator 
optimization. Although pole placement techniques in general 
are based on a single input signal, two inputs can be 
handled by the assignment of some weighting value (i.e., 
equal) ineffective towards the outcome. The effect of the 


two signals, y(k), now becomes, 


i2 


Y(K) = [laa vcres 
so that the gains for the two-input system become 
Kou=» (ieee 


where Ke are found via pole placement using ye(k). 

The inclusion of the observation (and later parameter 
identification) algorithms required the use of a slower 
sample rate, 5 Hz. See enclosed loop poles of the 
observer were chosen aS a compromise between fast response 
and the resulting sensitivity to extraneous noise when 
implemented as a model based compensator. The following 


were the final selection, 
Ko = [0.0;-0.4029;0.5677] 


F. PARAMETER ESTIMATION OF SENSOR BIAS 

Parameter identification of the rate gyro bias was 
accomplished by the use of an ARMA model of the gyro system 
itself where the input variable was the dive plane angle and 
the output was pitch rate. The ideal transfer function 
between input and output is a first order system which has 
two numerator coefficients, and two denominator coefficients 
to meneiry. (Note that the z-domain transfer function is 


to be used because we are dealing with sequences of sampled 


Iss 


data.) With this form, the parameter vector and the data 


vector can be written as, 
a — seca Ounoieie xX = [y(t) y(t-1) a(t) a(t-1)]; 
and 
6*x = bias(t). 


The foregoing is then repeated and a least squares fit 
for 68 is then obtained after the bias is included inside the 
vector %. Least squares solution of these equations yields 
both the vector of parameters, °, and the unknown bias. In 
this work, a recursive least squares algorithm was used with 
a Householder routine to compute the recursive gains which 


is time consuming but robust. 
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Til. ANALOG SIMULATOR 


A. GENERAL 

Simulation, a must for prototype development, provides a 
source of numeric, kinematic and logical awareness which is 
vital in designing a complex vehicle system and analyzing 
its performance. It provides an environment in which the 
many possible states of the system, and the transitions 
between them, can be aie and observed in an accessible 
and controlled manner. Properly utilized, simulation will 
reduce significantly the amount of field testing required. 
Design and performance deficiencies can be identified early 
in the development cycle, before the system is deployed 
Arthouse) 

A technique known as "Hybrid" simulation employs a 
combination of analog and digital computing [Ref. 13]. 
Using the hydrodynamic coefficients of the AUV model, the 
analog computer can be "programmed" to simulate AUV vehicle 
dynamics. With the control program running and a "target 
depth" entered as a keyboard input, AUV pitch angle, depth, 
and pitchrate, calculated by the analog computer, are fed 
back to the digital computer. The three states are compared 
to a model reference and an appropriate dive plane command 


is determined. The result is a continuous update (real-time 


AK 


control) of vehicle dive plane angle (see Figure 3.1 for 
hardware configuration). 

In the second phase of program development, the analog 
computer was configured to sustain a constant or variable 
bias on the system. This lead to the development of a bias 


estimator. 


ES DESCRIPTION OF ANALOG SIMULATOR 

In developing the analog simulations, the equations of 
motion of the vehicle are required to be converted. 
Convenient conversion or scaling is based on the maximum 
variable values being scaled with a +/- 10 volts maximum 
output on each amplifier. In analog simulations, variable 
summations and integrations are performed by summing and 
integrating operational amplifiers. Multiplication by 
constant coefficients are performed by attenuating potenti- 
ometers set to the appropriate fraction of 1.0. With 
scaling consistently applied through the 1 or 10:1 input, 
appropriate gains are sent to the corresponding amplifiers. 

Figure 3.2 shows an example of a first order system 
Simulation using an analog operational amplifier. The 


equation is: 


gf = [1.5] 6 rad/sec 
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DELTA (13 


(10 VOLTS = 1 RAD) 





Figure 3.2 Example of a First Order System 
Using an Analog Operational Amplifier 


where 6 is in ee Note the coefficient 1.5 rad/sec per 
radian is set at 0.15 in the potentiometer with a gain of 10 
in the integrating amplifier. 

In Figure 3.3, the diagram represents the third order 
model of the vehicle given by Equations in II.C. Table 2.1 
gives a listing of the system parameter values, their 
dimensions, and the potentiometer settings used in the 


simulations. 
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Figure 3.3 Vehicle Simulation Analysis 
Using an Analog Computer 
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IV. DIGITAL CONTROL PROGRAM DEVELOPMENT 


A. GENERAL 

As explained in Reference 7, this digital control 
program represents a functional component at the lowest 
level of the digital autopilot echelon. The digital control 
program interfaces with the servos controlling the AUV dive 
planes. The "handshaking" of data between the digital 
controller and the voltage-operated servos onboard the AUV 
model are accomplished by A/D interface hardware [Ref. 14]. 
In Figure 3.1 the hardware arrangement using the analog 
computer as a Simulator of sensor feedback is exhibited. 

Much of the program development of Reference 7 is 
implemented into this thesis. A number of dilemmas had to 
be overcome for program progression to continue. 

The preliminary program code had parameter conversions 
of feet/sec, inches, and degrees/sec. To alleviate valuable 
computation time, and to better support the AUV model, the 
code for the input and ouput (1/0) dimensions was modified. 
All I/O parameter dimensions were converted to voltages. 

Significant concern focused on the programmable inter- 
Gupineececncerol . Previous work was hampered when software 
interaction waS unachievable. The preliminary control 
program would run only when initial keyboard inputs of 


target depth were entered. Pre-programmed keyboard 
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interrupt modes would not respond to subsequent inputs. 
Keyboard interaction was masked and additional dive plane 
commands could not be executed. The program could terminate 
only if the CTRL-BREAK key combination was executed. 

From Reference 11 it was discovered that by removing the 
influence of the CTRL-~BREAK character at the start of the 
programming code, programmed interrupts could be invoked. 


By overcoming this impasse, program development was 


inevitable. Significant algorithm progression followed. 
"Canned" maneuvers, invoked by keyboard inputs, were 
developed and tested. Data collection and filing were 


implemented by actuating function key inputs. 

The final phase of research was committed to the 
development and implementation of an Estimator/Observer 
control program. This algorithm would reconstruct the state 
variable, pitch angle, using depth and pitchrate sensor 
feedback. Bias created by the relatively inexpensive 
onboard sensors was determined by a Kalman filter and 
removed from the system. A cleaner, more robust control 


Signal resulted. 


B. A/D, D/A CONVERSION 

The Analog to Digital (A/D) and Digital to Analog (D/A) 
conversion process is vital to the operation of this control 
system. Analog sensor signals from the AUV are digitized 
via the A/D process prior to entering the computer control 


program. Likewise, digitized command signals are converted 
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to analog signals via the D/A process prior to return to the 
dive plane servos. For the preliminary AUV autopilot design 
described in Reference 7, the total cycle time for a 
complete I/O conversion process was 5 milliseconds with a 50 
millisecond sample rate (20 Hz). In later paragraphs, 
discussion will focus on the need to decrease the sample 
rate in order to augment the larger, more complex control 
code. 

The digitizing process is accomplished by the Data 
Translation (DT) board, DT 2801-A. It is installed internal 
to the computer as explained in Reference 7. The board 
Sontiguration can be found in»sthe technical reference manual 
meet. 14}. All conversions on the DT 2801-A board are 
controlled by writing data to, and reading data fron, 
registers on the board. PCLAB subroutines is an interfacing 
software package that coordinates this task [Ref. 15] 

Conversion of a voltage value to a digitized value 
depends on the number of bits of resolution used by the 
converter. For instance, a 12-bit converter has 4096 Number 
Of Codes (NOC). For a bipolar range of +/- 10 volts, a 0 
NOC would correspond to -10 volts, while a 4096 NOC would 
correspond to +10 volts. An NOC of 2048, similarly, would 
correspond to 0.0 volts. 

Before these digitized voltage values are usable in the 
software control program, they need to be scaled to an 


equivalent numerical voltage value. Equations found in 
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Appendix D of [Ref. 15] were implemented into the control 


code for such a reason. 


em REAL-TIME CONTROL. CODE 

The control program code was written in the programming 
language Turbo Pascal version 3.0. A more proficient 
version (V4.0) is presently available, but it is not 
currently supported by the interfacing software (PCLAB). 

A real-time controller is "real-time" up to the limits 
of the software program implementing it. A significant 
advantage of Turbo Pascal iS operation speed. Reducing 
large problems down into smaller, easier defined procedures 
affords faster execution. Each procedure can then be 
subdivided further, at the discretion of the programmer. 

The control program executes an AUV dive control system 
within a user friendly multiple menu-shell setup. When 
running the compiled control program, the first scum 
encountered is the Main Menu. Here, the user is presented 
with a display of program titles and the options to quit or 
continue (run). The next screen offers a similar choice, 
where depressing the function key Fl starts the control 
mode. In this mode, the user is asked to name a "data file" 
(standard IBM DOS file). The results of the vehicle 
dynamics during the course of the run will be stored in this 
file. The final input screen prompts the user to "ENTER THE 


TARGET OPERATING DEPTH." Completion of this procedure 


ae 


activates the closed-loop dive plane control. The Run-Mode- 
Sereen is introduced on the monitor displaying updated 
values of AUV depth, pitch, pitchrate, dive command angle, 
sensor bias, and attitude. 

During closed-loop control, the user has four methods of 
operation: 


- F3--exit from the "Active Control Mode," and file close- 
out. 


- F2--activates a pre-programmed "Canned" maneuver. 

- Fl--exit from the "Active Control Mode," close-out 
previously selected file, creates new file with user 
interaction, and requests new target depth to reactivate 
closed-loop control. 


- Any alpha/numeric key--temporarily exits from "Active 


Control Mode," prompts user for a new target depth, 
while retaining previously selected data file for data 
storage. 


Two separate control programs were used to establish 
data collection and analysis. The first, AUVCOP3F.PAS, was 
designed as a closed-loop dive control system with full 


state feedback. The "COP3" corresponds to the third copy or 


generation control program. The "F" coincides with data 
Filing capabilities. "PAS" is Turbo Pascal's standard file 
type designation for programs to be complied. The second 


program, AUVCOP4F.PAS, implements an Observer/Estimator to 
reconstruct an unknown state variable from two sensor 
inputs. Sensor Bias 1S also estimated and removed from the 


system. 
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De SAMPLE RATE CONTROL 

A signal generator excites the DT 2801-A board to pass 
data to and from the data registers. The dial setting of 
the generator determines the rate at which this is 
accomplished. As mentioned earlier, a sample rate of 20 Hz 
was used in Reference 7. 

When running the full-rate feedback control program 
(AUVCOP3F.PAS) at 20 HZ sample rate, Turbo Pascal computed 
the relatively small algorithm in 5 milliseconds. Manually 
reducing the sample rate during program runtime resulted in 
a stable controller as low as 3 Hz. An ideal condition, 
though, is to maintain a high sample rate to yield a more 
stable, faster responding controller. 

The Estimator/Observer control program (AUVCOP4F.PAS), 
however, contains an additional 400 hundred lines of code. 
Here, computation time was increased significantly (greater 
than 150 milliseconds) and could not keep pace with the 20 
Hz sample rate. To accommodate, the sample rate was reduced 
to 5 Hz. Likewise, new gains were derived. 

Current hardware/software configurations of the AUV 
controller are limited in performing these multiple matrix 


calculations at high sample rates. 


E. SOFTWARE LIMIT FOR ACTUATOR SATURATION 
Present hardware configurations on the 30 inch model 
required small input voltages to actuate the full range of 


the dive plane angle. During previous research of open-loop 
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tests, dive plane angle was compared with the corresponding 
input voltage. Results concluded that a linear 
proportionality on the order of 0.01 volts per degree dive 
plane angle existed. This was true for a range of 0.0 to 
10.0 degrees. Hence, the operating range of the dive planes 
became +/- 10 degrees, and respectively, the operating 
voltage range became +/- 0.1 (using the hand-held 
transmitter). The dive plane voltage range of +/- 0.1 was 
implemented sate the analog computer configuration. The 
control code was changed by creating a software stop prior 
to sending the final command signal to the plane. This was 
implemented by limiting the final signal sent through the 


D/A conversion to +/- 0.1 volts in the forn, 
if abs(u) > 0.1 then u = 0.1*abs(u)/u. 


F. FILES FOR DATA RECOVERY 

During the open-loop vehicle tests, data acquisition was 
achieved by using the software program DT Notebook [Ref. 5] 
on the IBM PC/AT. The computer's system configuration file 
had to be specifically structured for DT Notebook operation. 
However, the programming language Turbo Pascal used an 
entirely different configuration. The two programs, 
therefore, could not be run simultaneously. In other words, 
data acquisition by DT Notebook could not be performed while 


running the closed-loop control program. 
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An alternate method of data recovery was devised using 
Turbo Pascal [Ref. 11]. The keyboard commands for data file 
use are found in Chapter IV.C. Although additional through- 
put is needed for data file simulation, the time used in 
writing data to a hard disk was found to be negligible in 


comparison to the sample rate. 
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A. GENERAL 
In Figure 5.1, the closed-loop full state feedback 
system is displayed in block diagram convention. re 1s 


comprised of the system model: 


y 
C |——+ 


Yr 








Figure 5.1 Full State Feedback Diagram 
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where the input of target depth is the set point r, the 
closed loop gains are Kc, and the setpoint matrix N is 
designed for zero steady state depth error. The following 
sections in this chapter evaluate the response of the above 
system under varying conditions. The gains are fixed and 


the sample rate is 20 Hz. 


Be RESULTS OF FULL STATE FEEBRBBACK 

In the full state feedback controller, the closed loop 
gains are designed for an AUV speed of 2.1 Ft/s. In the 
following figures, AUV depth voltages ranging from 0.0 to 
10.0 volts represent the water depth from the surface to a 
depth of 10.0 feet below the surface respectively. The dive 
command voltage ranges from-+/- 0.1 volts equivalent to a 
Give plane angle of +/- 10 degrees. In Figure 5.2, a target 
depth of 6.0 volts becomes the new set point from 1.0 volt. 
Subsequently, a dive command of 10 degrees is sent to the 
Give planes. Almost immediately, the AUV responds and 
levels out at the new depth. The entire "run" lasts 
approximately 120 sample increments or six seconds, as 
expected, with an overall dominant time constant close to 


the 1.0 second used in the gain design process. 


C. EFFECT OF ACTUATOR SATURATION 
As previously mentioned, the dive plane actuator is 
limited to a range of +/- 10 degrees. Tf this limit is 


removed then the results of Figure 5.3 are found. Comparing 


32 


0°SL>G 


anzey etTdwes zH oz pue s/3a Zz FO psasds 
e Ze 3997 9 OF T WOAF AsAnNsueW YAdeqG 


(ONS GO’ AO SLNAWANONDAWLL 
0°OGeS O'Sce 0°O00c¢ 


ZHULddd ANV 
QNAOaT 


O'SLT 





Oc- 


O'l- 


00 


SLTOA-GW9D ANV Td AAIG ? Widdd ANV 


OT 


Oe 


Ob 


OV 


0-9 


a3 


ayey stdwes zy oz 3e S/aa Z ipeeds -ssaurtad SATA 


UOTFEANAFeS YNOYRTM 4997 G OF T WOT ASeAnsuey yjadag 


(94S GO’ AO SLNAWAYONDAWLL 
0'SAG 0°OS?S 0°SGG 0'00c O°SLT 


t 


t) 
CROSSES HSS SHS HR SHEESH HSHSHSHEHF SCHHEHTHHSHSHSHHSHSHHSHHSHRHHSHRHEHSHRHEHHA MOORES HHEHRH MSH HEHEHE HOHE HSHREHEHHEHHERESSSHSHS SHEESH HEHEHE HEHEHE EF SHEESH HERR HEE 


e 
See eee eee eSBs SSeS HSeeeFeFSSsHeeSHeS Sees Heeme sees sees eeeeeeeeehoaqpeeeeseeaezeeeeeeeeeeeeeseeeeeeepeseeeeeseeneveneann 





ZHiddd ANV 
CNH T 






2syuq UO 


O°OST O°SaT 
} 


eCeeawr een eee meee eeeeeeeateeatoaent®eaeneeeeaene 


CORRS SOSH MESSRS HSH EHHOHSHH AEH HR EEE DO*E 


@eeeeeneteemoeseeaeeeeeeeeeeeeeeteaetanenen 


0 OT- 


09- 08- 


SLIOA-GWO ANVW Id HAIG 7% Woddld ANV 


Oe 0-0 0c- O06 3 


Ov 


0'9 


€e°sg sandSty 


34 


this to a second run, having actuator saturation, the 
results of Figure 5.4 show that the non-saturated dive 
command achieves a slightly faster AUV depth response. 
Figure 5.4 attains 5.0 volts in approximately six seconds 


whereas Figure 5.3 attains target depth in approximately 5.5 


seconds. inenreubesmoe> andss.6,,a Similar comparison is 
made. Here, however, the depth change 1s considerably 
smaller. Similarly, the response time difference is less 
noticeable. 


As one would expect, a larger actuator in the dive plane 
drives a quicker AUV response. While actuator rate-of- 
change limits may be an additional limit to consider, these 


results assumed that such a limit was not present. 


be EBEFECT OF VEHICLE SPEED 

Though the closed loop gains were designed to operate 
the AUV at a speed of 2 FT/s, other speeds were tested to 
determine the robustness of the controller so obtained. In 
Figures 5.7, 5.8, and 5.9, the vehicle speeds of two, four, 
and six FT/s respectively were evaluated when the AUV depth 
@nanged from six to four volts. Mimetticgwurne. 5.7, =the AUN 
smoothly reaches the assigned depth without overshoot in 
approximately five seconds. In Figure 5.8, target depth is 
attained in nearly 3.5 seconds with a slight overshoot 
before leveling out. Finally, in Figure 5.9, overshoot is 
most noticeable, but target depth is accomplished in close 


to three seconds. Based on these observations, it can be 
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noted that through a 300% change in the speed parameter, the 
closed loop control performance remains stable with only a 


40% change in response time and, for overshoot, a change of 


less than 15%. 
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VI. BIAS ESTIMATION AND RESULTS 


A. GENERAL 

In this chapter, the final stages of program design are 
implemented. The results are graphed and evaluated. Here, 
the fullstate feedback controller in Chapter V is combined 
with a parameter estimator and state observer system as 
shown in Figure 6.1. The appendix details this program 
code. 

The parameter estimator determines sensor bias of the 
depth pressure cell and pitchrate gyro. The three-state 
observer determines the missing state, pitch angle, and 
filters the return signal of the remaining two states. 

With the inclusion of the above algorithms (over 400 
lines of additional code), the 20 Hz sample rate or 0.05 
seconds per sample, had to be reduced. Significant increase 
in computation time prevented a sample rate much higher than 
5 HZ or 0.2 seconds per sample. Hence, the new sample rate 
became 5 Hz and the poles in the z-plane were changed 
accordingly. The remaining figures in this chapter show the 
results of the new code at work. For purposes of 
evaluation, vehicle depth change remains constant while 


other parameters are varied. 
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command input 









PARAMETER 
ESTIMATION 
by 


bias estimate 


Diagram of the Bias Compensated Model Based Controller 


Figure 6.1 Diagram of the Bias Compensated 
Model Based Controller 
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B. EFFECT OF SPEED CHANGE ON AUV RESPONSE 

In Figure 6.2 at an AUV speed of 2 ft/sec, a smooth dive 
maneuver from 1.0 volt to 5.0 volts yields a response time 
of 13 seconds, as compared to six seconds in Figure 5.4. 
The decrease in sample rate has moved the poles, retarding 
AUV response time by more than a factor of two. 

In Figure 6.3, the vehicle speed is doubled to four 
ft/sec and the same maneuver is invoked. Here the depth 
response curve is not as smooth or direct, but no overshoot 
is observed. Since the poles are designed for a slower 
speed, the vehicle tries to settle out prematurely, delaying 
the depth change process. This maneuver was accomplished in 
7 seconds. 

Results of this vehicle speed increased to six ft/sec 
are shown in Figure 6.4. Again, the curve is not smooth, 
with abrupt settling occurring after two-thirds of the depth 
Smange and then continuing on to 5.0 volts depth. The 
completion time for this maneuver is greater than 20 
seconds. 

For each increase in speed, maneuver completion time is 
extended, but target depth overshoot, however, does not 


occur. 


C. AUV RESPONSE TO A BIAS SIGNAL 
As mentioned in preceding chapters, the return signals 
from the relatively inexpensive sensors onboard the 30 inch 


model are prone to include sensor bias. In the subsequent 
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O'S 


figures, the results of vehicle depth with sensor bias is 
shown. A 0.1 volt sensor bias in Figure 6.5 results in a 
corresponding 0.1 volt decrease in actual depth as compared 
to the target depth of five volts. Although maneuver 
completion time is not influenced (equivalent to Figure 6.2 
With zero bias), the target depth is never quite reached. 
Similarly, in Figure 6.6, a larger sensor bias of 0.2 volts 
results in a vehicle depth falling short of the target depth 
by the same amount. 

By slightly adjusting the program code, the estimated 
bias can be removed from the system resulting in a cleansed 


depth response signal, shown in Figures 6.7 and 6.8. 
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VII. SUMMARY AND CONCLUSION 


A. SUMMARY 

This thesis presents the implementation of a real time 
autopilot dive plane controller for the NPS AUV testbed. 
The controller includes a state observer and sensor bias 
estimator. The approach to the design of the autopilot 
mec ludes: 


-~ Manipulation of vehicle dive dynamics to develop 
linearized equations of motion, 


- Implementation of an Analog Simulator using data from 
open loop control. This affords more effective and 
efficient development of the full state and partial 
state feedback control algorithms, 


- Hardware/Software interface via A/D and D/A conversion 
boards, 


- Program development using Turbo Pascal version 3.0, 


- Sample rate control on program code of differing 
magnitudes, 


- Incorporating data recovery via file opening techniques. 


B. CONCLUSION 

This study developed a dive plane autopilot for the NPS 
testbed that showed signs of robustness. Under closed loop 
control, the vehicle stabilized at various depths under wide 
ranges of speed with no overshoot. 

As program code increased in size, throughput increased 
reducing sample rate, and therefore, reducing autopilot 


response time. Future research of the steering control 
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autopilot may need to optimize current algorithms to prevent 
further throughput increase and to optimize robustness 
characteristics. Parallel-tasking processors may be an 
option to large, complex, programming code, creating a 


faster throughput. 
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APPENDIX 


CONTROL CODE FOR AUTOPILOT 





Se), 

program AuvAutoPilot ( input, output }; 
TITLE : Autonomous Underwater Vehicle Auto Pilot Program. 
AUTHOR ¢ LT Jerry «@. Reina Us 


APPLICATION : real-time controller with a Model Based Compensator to 
estimate pitch angle, and a parameter estimator to determine 
sensor bias 

DATE : 8 Dec 1988 


Project Description : This program implements digital control of the NPS 
autonomous underwater vehicle (AUV) in the vertical or dive plane.It samples 
vehicle sensor input from two channels : depth and pitchrate. The Model 

Base Compesator determines pitch angle. All states are then multiplied by 
the controller gains and compared to the target depth. An appropriate dive 
command is determined and sent back to the vehicle via D/A conversion boards. 
A deflection on the dive planes result. The sample rate is 5 Hz.) 


{ GLOBAL DECLARATIONS =—-======"==—2-----—-——-_--—-—---_-~..---—~—.,__ _ > ee } 
const 
Cae ie Sereen declarations. ._-_-_--—-—_— } 
x1 =, { Upper left corner : left edge J} 
yi =n 2 { Upper left corner : upper edge } 
x2 = 75; ( Lower right corner : right edge } 
y2 = 24; { Lower right corner : bottom edge } 
type 
str10 = string [10); 
str60 = string (60); 
var 
hr, ne 2m i, m1 2, 
sec,sec2,hun,hun2 : byte; 
seconds : real; 
option, controlmode, 
reply,reply2 : char; 
i INCLUDED FILES Declarations -----------—=3..—08 } 
{$I pcldefs.tp ) { PC LAB Trubo Pascal routines. } 
{$I pclerrs.pas } { PC LAB error code messages file. } 


{S$Ildrawbox2.auv} 


{ 


Input xl,yl,x2,y2 : integer to specify the corner limits of the box. 


This procedure clears screen and draws a rectangular box of specified 
dimension using ASCII double line characters. } 


{SiIclrbox2.auv)} 
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feline xl,yl,x2,y2 : integer to specify the corner limits of the box. 
This procedure uses a FAST means of clearing a box of specified dimension. 
The box dimension should be delcared as constants. } 


{$Iboxprint.auv } 
{ Input the printrow, leftboxedge, rightboxedge : integer and printstring 


str60. This procedure centerprints the string in the box at the printrow 
specified without overwriting the box border. } 
{SIshowfast.auv} 
{ Input message : str60, column,row : integer. To specify the x,y position 
on the screen for a FAST message print. ) 


{Sikeyhit.auv} 
{ This is a boolean function which returns true or false If key is pressed; 
it also returns keycode replies VAR reply, reply2 : char .. } 


{Sltabxy.auv} 
{ Input tabcol,tabrow : integer; like gotoxy } 


{$Ilboxpause .auv} 
{ Input xpause,ypause : integer to specify where "Press any key to continue" 
message is to be printed. } 


{S$Ilgetkey.auv } 
{ Input as a string of chars, the set of acceptable replies; ie 'YyNn'. This 


procedure waits until one of the acceptable replies has been entered. } 
[ KKKKKKKKKKKKKKKKK MAIN PROGRAMS PROCEDURES RHR KK KKK KEKE KEK KK IKK KKK EEK KKK) 
{ KRKKKKKKKKKKKKKEKK USER INTERFACE MODULES KH KKK KK KKK IKK KEK KK KKK KKKKKK ) 


procedure MainMenu ( var reply : char }); 


{ This procedure presents the AUV screen and solicits an option to Run 
the AUV from the StatusAndCommand procedure or to Quit. } 


begin 
repeat 
elyrser; 
drawpex7(xivvl,*¥2,y2); 
"N 


boxmprine(yi+3s,xl,x2, ReY AL POSTGRADUATE Se@ H OMOel' ); 
Dore umimwuyl+s,xXl,x2,°D E.b AgRoT M EN T Oa); 
boxprint(y1t+6,x1,x2,'ME CHANICAL ENGINEERING'); 
boxprint(y1+8,x1,x2, ‘AUTONOMOUS UNDERWATER VE TChE y) : 


boxprint(y1t10,x1l,x2,'DIGITAL AUTOPILOT CONTROL PROGRAM'); 
boxpmamt (yl+12,x1,x2, PKK KK KK KKK KK KKK RHEK KKK KKK KKK KKK KEEKKEEKK! : 
DOswPEi nt tiyitlo,xl,xe, Do You want to RUN this program ..'); 
boxprint(y1t16,xl,x2,'or Do You want to QUIT and return to DOS ?'); 
boxprint(yl420,x1,x2,'>>>> ENTER Q OR R <<<<');3 
getkey ('QqRr',reply,reply2); 
ieee r iyi. © > 'q', "RY, Ey) aram(reply2 = chr(0)); 
end; 
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procedure StatusAndCommand ( var mode : char ); 
{ This procedure begins the control program screen. } 


var 
mode 2 : char; 


procedure StatusAndCommandScreen; 
{ This is the status and control screen and solicits a user input of Fl to 
RUN the program or Q to Quit and exit to the main menu. } 


begin i oe ie StatusAndCommandScreen ------------------- } 

clrbox2 (xl,ylex2 7,42); 

boxprint(yl#1,x1,x2,'AUV STATUS / COMMAND AND CONTROL SCREEN'); 

boxpr int(yl ery: x] xX L5 ee ee ee ee ee 2 2 2 2 2 2 2 ee 2 2 ee ee ); 
boxprint(y1+7,x1,x2, ‘CHOOSE YOUR DESIRED CONTROL MODE =:;,"); 
boxprint(yl+9,x1,x2,'ENTER KEY << Fl >> TO START AUV CONTROL'); 
boxprint(yl+11,x1,x2,'ENTER << Q >> TO QUIT AND RETURN TO MAIN MENU'); 
boxprint(y1+16,xl,x2,'PRESS EITHER Fl OR agi); 


end; { “Steep eee aoe StatusAndCommandScreen -------------—— as } 
{ KKK KKEKKKKKKKKKEKKEKKEKKK CLOSED LOOP CONTROL ROUTINES ® 822K RKKKKKKKREKKKKKK 3 


procedure ClosedLoopControl; 


{ This module comprises the closed loop control scheme. ) 
labelaeo: 
LABEL 1; 
const 
maxdepth = 33; 
mindepth = Q; 


updateincrement 10; 
type 
try = array([1..10} of real; 
activecontrolmode = ( run, reset, exit ); 
allowabledepthrange mindepth..maxdepth ; 


auvattitude ( climb, maintain, diving ); 


Gigitalintegerarray = array [1..3} of integer; 


var 
filename:string{14}; 
fFilevar:text; 
auvdepth, auvdepthvolts,auvspeed,auvpitch,auvpitchvolts,auvpitchvoltsxx, 
auvspeedvolts, auvpitchrate, auvpitchratevolts,estdepth,err, 
deptherrorvolts, targetdepthvoits ,biasi,divevolits,tgtnew,targetdepth,speed:r 
eal; 
adv : digitalintegerarray; 


j,status,time : integer; 
modereply,modereply2 . Chtak: 

activemode : activecontrolmode; 
updatecounter, initial -: Integer; 

depthrange : allowabledepthrange; 
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attitude ; auvattitude; 


acl :real; 
ac2 nea) : 
ac3 :real; 


procedure GetTargetDepth (var tgtdepth : real ; 
var tgtdepthvolts : real ); 


{ This procedure solicits the target AUV operating depth and converts it to 
an AUV eguivalent targetdepth analog voltage and passes both of these 
parameters. } 


hegIn (GQRa aS = ee reer eee ee ) 
mumuox2 (%1,y1,xX2;,y2); 
Deore ne. yr110,%1,%2,' ENTER THE A U VoTARGET,OVERATING DEPTH); 
boxprint(yltil,x1,x2,'NOTE : THE DEPTH SHOULD BE IN VOLTS (0.0 - 10.0)'); 
repeat 
begin 
boxprint (y14+13,x1l,x2,'ENTER TIE TARGET OPERATING DEFTH '); 
dotonyetn ls 33, ylt+15)5 
read ( tgtdepth ); 
end; 
until tgtdepth >= 0.0 ; 


tgtdepthvolts := tgtdepth ; 
end); (ee Se ACLS eM He ae NEG) YE) OG |e I I aaa a aad } 
procedure RunModeScreen; 
{ This procedure displays the Closed Loop Control Screen in the RUN MODE. } 
begin Saaaa a > > Renae dem creer = —— ===> —— >> =a — ) 


eibapoy 2 (Xl, yl,x2,y2); 
DOxpLinct (yeti xl «2, 'A UV Sieaniwet OUS Jame O Ne Ter O Lb Ys7e R E E N*) 


boxpr int(y] Tae MZ Veer rn Se Set Pe StS ABS HSS KH PS KES SEs SH eer resets essere seerece! he 
boxprint(yl+4,x1,x2,'STATUS OF A U V OPERATING PARAMETERS :'); 

write (tabxy (x145,y14+6),'AUV DEPTII tvoursim: '); 

Gime (tabxy (x1+5,y1+7), ‘AUMePITEH Fvoltsio :'); 


write (Cabxya@xl45,y14+8),*AWNM@PITCHRATE [volts) : '); 
write (tabxy (x145,y1+9),'AUVDIVEPLANE {volts) 3: '); 
VTi Ue eGabey (x1? Soy1010), \BLAS= : 

Doxor inti, xi,x2,'A I V CONTROL STATUS :')}; 


write (tabxy (x145,y1+13),'CURRENT TARGET DEPTH : ') 
write (tabxy (x145,y1414),'CURRENT MODE Scanlan) 


ss 


write (tabxy (x145,y1415), 'CURRENT MANEUVER ay 


boxprint(y1418 > xt 


"PRESS KEY Fl .. TO ENTER NEW TARGET DEPTH. ‘yi 
boxprintilyvit 13 tx. 

‘PRESS KEY F2 .. TO STOP ACTIVE CONTROL AND RESET. ); 
boxpr int (y11207 3 

"FRESS KEY F3 .. TOVEXIT ACTIVE CONTRCE: — 
end; ee RUNnMOdescr Cen ~~=— 3 } 


procedure UpdateRunModeScreen (updatedepth,updatepitch,updatepitchrate 
,updatedivevolt : real; 

updatetargetdepth : real; 

updatemode: activecontrolmode ; 

updateattitude ;: auvattitude; 

var biasl: real); 
{ This module updates the Closed Loop Control Run Mode Screen with updated 

display parameters. Updates occur in intervals specified by updateincrement 


interval declared in ClosedLoopControl procedure. ) 
begin 10) 6 ae ree UpdateRumModeS@reen 9 ~~ = a en = ee } 
{ UPDATES STATUS OF A U V OPERATING PARAMETERS } 


writeln (tabxy (x1437,y1+6),updatedepth:6:3); 
writeln (tabxy (x1+37,y1+7),updatepitch:6:3); 
writeln (tabxy (x1137,y1+8) ,updateptcenrate: 6.47 
writeln (tabxy (x1437,y1+t9),updatedivevolt:6:3); 
writeln (tabxy (x1+37,y1+10),bias1:8:6); 

{ UPDATES THE A U V CONTROL STATUS } 
write (tabxy (x1+30,ylt13),updatetargetdepth:6:2); 
case updatemode of 

run..: writein (tabxyo(x14130;, yi4 RUN 
reset: writeln (tabxy (x1+30,y11+14), 'RESET' ); 
exit. ; writein (tabxy (x1430,y)! sie exe; 
end; 
case updateattitude of 


maintain : writeln (tabxy (x14+30,y1+15),'MAINTAINING DEPTH tones 
climb s: writeln (tabxy (x1430,y1t15),'CLIMBING TO TARGET DEPT 
diving >: writeln (tabxy (x14+30,y1+15),'DIVING TO TARGET DEPTH '); 
end; 
end; ts> > Seo Ss ae UpdateRunNodeSctreense-— —- sas =" 9 9 559-5 = 55 a 


procedure GetDigitalSensoryData ( var depthanalogvolts, pitchanalogvolts, 
pitchrateanalogvolts :real) ; 


{ This procedure uses PCLAB routines to sample selected input telemetry 
channels from the AUV and digitizes these inputs and multiplies them 
by the specified gains. 


DT 2801-A / DT 707 Board set up: channel 1 - AUV depth input 
channel 2 - AUV pttch input 
channel 3 - AUV pitchrate Input } 
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Cons c 


{ These are AUV to DT 2801-A / DT 707 hook up board channel configurations, 
conversion and computational arguments. 


Gepthchannel = 1; 
depthpfs = +10.0; 
depthmfs ="=107. 0; 
pitchchannel = 2; 
spdpfs = +10.0; 
spdmfs =o. 0; 
pitchratechannel = 3; 
pitchratepfs = +10.0; 
pitchratemfs = -10.0; 
noc = 4096; 


{ SetUpAdc and ADConTrigger 


{ AUV output to DT-707 input channel assignment 
{ Peak depth signal value 
{ Minimum depth signal value 


{ AUV output to DT-707 input channel assignment 
{ Peak pitch signal value 
{ Minimum speed signal value 


{ AUV output to DT-707 itnput channel assignment 
{ Peak pitchrate signal value 
{ Minimum pitchrate signal value 


{ Number of Codes; conversion resolution. 
The DT 2801-A performs a 12 bit conversion. 
NOC = {2 * conversion bits), te 4096 


PCL function arguments 
p 6-8 PCL documentation } 


boardnum = 9 
numa2dchan ee Bi 
timingsource = 2; { -- Sets a external trigger,internal clock } 
adcgainl = l]; 
adcgain2 = 2; 
adcgain4 = 4; { Sets the A/D gain; 1,2,4,8 are options 
} 
adcgain8 = 8; 
startchannel = 1; 
endchannel =— 35 
var 
pitchadyv, 
depthadv, 
pitchrateadv, 
signaladv, { Signal analog data valve } 
counter,status, 
chanum,i,3 : integer; 
begin { ------------ Procedure GetDigitalTelemetry ---------- } 


{ Set up the DT 2801-A board to take data. 


status := SelectBoard {boardnum); 


{ Set up the DT 2801-A board to take data from 3 input channels; Data sampling 
is initiated by the ADConTrigger single channel sample of the depth channel 
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and then single ADC value samples of the speed and pitchrate follow. 
The Trigger is connected to the DT 707 board at terminal 49 from a signal 
generating source. } 


status 
status 
status 


ADConTrigger ( depthchannel, adcgainl, depthadv ); 
ADCValue [{ pitchchannel, adcgainl, pitchadv ); 
ADCValue ( pitchratechannel, adcgainl, pitchrateadv ); 


{ Convert the digitized Analog Data Values for pitch, depth, pitchrate to 


analog voltage values. The algorithm for this conversion is found in 
Appendix D of the PCLAB documentation. } 
depthanalogvolts := ( depthadv * (depthpfs-depthmfs)/noc ) 


+ depthmfs; 
pitchanalogvolts := ( pitchadv * (spdpfs-spdmfs)/noc ) 
+ spdmfs; 


pitchrateanalogvolts := ( pitchrateadv * 
{(pitchratepfs - pitchratemfs)/noc ) 
+ pitchratemfs; 


{ status := Terminate; } { --- PCL routine, closing the DT 2801-A } 
end; { ------------ procedure GetDigitalTelemetry ---------- ) 


procedure ATTITUDE_ ( tdepthvolts, adepthvolts : real; 
var attitude : auvattitude lS 


const 
Gepthcontroltolerence = 0.1; 
modelgain = 1.0; { This simulates a model referenced } 
{ gain parameter ) 
Var 
voltsdifference : real; 
begin ki miei Errorvolts,=---==s2>---=>++4-——----—-->- 7 > 3a ae } 


voltsdifference := tdepthvolts - adepthvolts; 


{ 444444444 4+44+4+4444+4 Control voltage filter FHHHEHHEHA EEE AFH HA AFH FFE +H ett ttt 3 
{ These conditions check if depth is within tolerence. } 
if ( voltsdifference > 0) and 
( abs(voltsdifference) <= depthcontroltolerence ) then 


begin 
attitude := maintain; 
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end 
else if ( voltsdifference < 0) and 
( abs(voltsdifference) <= depthcontroltolerence ) then 
begin 
attitude := maintain; 
end 


Meinis condition checks 1f£ actual depth is less than target + tolerence.) 


else {f ( voltsdifference > 0) and 
( abs(voltsdiftference) > depthcontroltolerence ) then 
begin 
attitude := diving; 
end 


( This last condition checks to see if the actual depth is more than target + 
tolerence.  } 


else 1f£ ( voltsdifference < 0) and 
( abs(voltsdifference) > depthcontroltolerence ) then 


begin 
attitude := climb; 
end; 
end; ( ==SS5 > ee ee ee 


procedure EST(var up,yq,z:real;var vhat,pitch:real;var initial: integer; 
var bias,al,a2,a3: real); 
(This procedure uses a Model Based Compensator to determine pitch angle. 
A parameter estimation using RLS fit determines sensor bias.) 


label 3; 
type 
try = array({1..10) of real; 
Erytry = array(!..10,1. .140)})%@£ real; 
try40 = array[1..40,1..40) of real; 
tryt40 = array[1..40) of real; 
const 
ns = 1; 
var 


beta,g,khat,xdata,g2,phid,phigq, fc, phi2,khat2,xhat2,xhat, 
g3,h3,khat3,phi,phat,xu,xy,xz,xs,xnew: try; 

Spe, tpapee, ©3,S5pts = trytry; 

nx,np,ny,nl,nf,n2,nsn£,time,i,j : integer; 
zd,Z£,zf2,uf£,y£, fs,af,y, yhat,ud,u, yd,p0,sr3,sr2,8r,8q3,8]9,8q2, 
zdd,sg2,sigmax,gbias;: real; 
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procedure initializeArrays; 
begin 
for e7="1..to 1oOsdso 
begin 
beta 


=e %e Be We 


variance of the bias) 
sigqmax Oly 

sq := sigmax; 

sq2:= siqmax; 

sq3 := sigmax; 

Sr $= 7120; 


nnigoaogeaos 


~~ 


for {:= 1 to nx do 
begin 
for j:= Ll tornx do 
begin 
spttli,j) s= : 
sptZ2ti, J ioe 200. 
Flizgo) :=S0R0; 
£3(1,4):=0.0; 
1f !{ = j then 


0.0 
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Giiixte =" 41.0; 
G@zlz2):= 1.0, 
{fet rl — 42.9 2725-2; 


a 


fe(2)2="-G.201s3E-1 ; 
fegse— 1.3633E-1; 
fcot4):= -7.5566E-2; 
fol5).=70. 0; 
Perel so .c06l1h 2; 
fetrl:= 3.9545E-1; 
Fel 8) t= S.9317E-1; 
fcol9)J:= 3.9545E-1; 


: f€c(10):= 9.8861E-2;} 
fco{1):=0.80000; 
fco({2]:=0.20000; 


nf:= 1; 

fsa? = “5 70; 

£9(1,1):=0.6977; {discretized A-matrix) 
Boteeee li O.. 168 0% 


Pate, 27s =1.0; 
Pat 3,1):;=-0.01786; 
£33, 2): =-Oiee) ; 
fed, 3s =a 0 ; 
g3ti}:=0,4251; {discretized B-matrix} 
gpie2):=0.0451; 
g3(3]:=-0.0031; 
khat3{1):=-0.0073;khat3(2):=-0.1046;khat3(3):=0.4250; } 
'ga.ns for SHz.,potes@(0.75 0.76 0.77) kob7 } 
khat3(1}:=0.0;khat3(2]):=-0.4029;khat3(3):=0.5677; 
{kob8 @ ob8poles=[{.7 .71 .72] at 1lft/s) 
bias:=-0.0; 

end; 


procedure innerprod(var y9: realjvar phi9,x9:tryj;var nx9: integer]; 


var 
Jj: integer; 
begin 
Yow 0.0; 
For j e273. -t0 nx9 DO 
begin 
y9 t= sphagts) * x9(j 1+ y3; 
end; 
end; 


procedure update(var phi8: try;var u8,y8: real; ns:integer]; 


var 
n2:integer; 
i: integer; 
phisave: try; 
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begin 
n2¢=nS 224.0; 
for 1 3=) 0 nz tae 


begin 
phisave(iJ:=phi8(iJ; 
end; 
b= aS 
while 1 >= 2 do 
begin - 
phi8s(ijJ:= phisave(! - 1); 
phi8({ns + i} := phisave(nsti-1); 
; er es ec ie 
end; 
phis(1J:= y8; 
phi8{ns + 1) := u8; 
end; 


procedure filter(var u4,y4:realjvar x4,fc4:try;var nf4:integer); 
var 
nf£f42 : integer; 
savex4:try; 


begin 
update(x4,u4,y4,n£4); 
n£42°>= 2y* nes; 
Iinnerprod(y4,x4,£c4,n£42); 
{writeln('inslde filter after inner,y4,x4=',y4,x4{1],x4(2)],x4l{ 
2D} )h? 


end; 


procedure newest(var nx7: integer;var xhat7,khat7,h7: try; 
var y7:real); 
var 
delta: real; 
i,j: integer; 
xsave:try; 


begin 
for i:=1 to nx7 do 
begin 
xsave(i):=xhat7(iJ; 
end; 
delta := y7; 
for j:= 1 to nx7 do 
begin 
delta:= delta - h7{j} * xhat7(3); 
end; 
for i:= 1 to nx7 do 
begin 
xhat7(ijJ := xsave{i) + khat7{iJ) * delta; 
end; 
end; 
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procedure hholder(var nr6,nc6: integer;var m16: try40); 
var 
tT COtgt, J, ks wineeger, 
temp,sigma,sum : real; 
a6: tryt40; l[arrayl(1..410) of real;} 
c6,h6:try40;{ array(1..40,1..40] of real; |) 


begin 
for col:= 1 to ncé do 
begin 
sum := 0.0; 
for j := colm@uo nré do 
begin 
sum:= sum + m16(j,col)*mi6(j,col}; 
end; 
sigma:= sqrt(sum); 
for, J°="Col to, nre do 
begin 
a6(j7)2=" 0.0; 
end ; 


a@6{col):= sigma; 
sum := 0.0; 
For j:= col to nr6 do 


begin 
sum := sum + sqr(ml6[j,col)-d6[j)); 
end; 
for j:= col to nré do 
begin 
for k:= col to nr6 do 
begin 
begin 
{ff abs(sum) < 0.00001 
then temp:= 0.0 
else temp := -2%(m16[j,col)-d6[j]))*(m16[(k,col)- 
G6(k))/sum; 
end; 
if j = k then 
begin 
temp:= 1.0 + temp; 
end; 
h6(j,k):= temp; 
end; 
end; 
FOr. jJ:= col to fr6"do 
begin 
for K:= col tounes do 
begin 


temp:= 0.0; 
for 1:= col to nr6 do 
begin 
temp:= temp + h6[(j,1)*m16[1,k]; 
end; 
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c6{3,k)2= tenp- 
end; 
end; 
{ meo:= c6;} 


for t:=col to nr6é do 


begin 
for j*=ecor to nesb do 
begin 
mori, . 2 -Col to 
end; 
end; 
end; ; 
for i:= 2 to nr6 do 
begin 
FOES) := TO 1 -LPedo 
begin 
mi6t!,jJ):= O30; 
end; 
end; 


end; 


procedure kgain(var nx5: integer;var spt5,£5: trytry; 
var g95,hS5:tryj;var sr5,sq5:realjvar khat5: try); 
var 
ny, i,j,nu,k,nytot,nx5nu: integer; 
mi5: try40; 
Sptt Stevery: 


begin 

mi5({1,1):= sr5; 

ny:= 1 ; 

nu:= 1; 

for i:= lvtomnxsedco 

begin 
for j:= 1 to nx5 do 
begin 

miS(ny we nyte: = sptst),1); 

end; 

end; 

for i:= 1 to nx5 do 

begin 
miS(nyt+i,ijJ:= 0.0; 
for k:= 1 townxs do 
begin 

miS(nyt+i,l1J:= mlS5(nyti,ll+spt5tk,i)*h5(k); 

end; 

end; 

for j:= nyt1l to nytnx5 do 

begin 
mi5(1,j):= 0.0; 

end; 


nytot:= nyt+nx5; 
{ writeln ('enter hholder first time'); } 
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few rere rm ( mis'’,ml5{l,)],ml5(1,20;m15(1;3)); ) 
hholder(nytot,nytot,m15); 
(vwmarGemm@e misatter hholder £rst ',m15(1,1),ml15(1,2),m15(1,31)3) 
(RRRRKKEKRKEKREREKEKEREKRREEKREKEKEEEKREKEKEKEKREKREKEKE | 
if abs(m15(1,1)) <= 0.0000001 then writeln('R is singular'); 
For iL:= 1 to nx5 do 


begin 
Kiagesi is= Milo ti,i+2)/mis(i,1.); 
( weittein('’kKhats', khatsti), ye) 
for j:= 1 to nx5 do 
begin 
SHtel sy, ese milo timytii,nytj]s 
end; 
end; 
FOr 1:= lveornx> ao 
begin 
for j:= 1 to nx5 do 
begin 
Mol tous = 0 20> 
FOLK :=  1ecoen<> do 
begin 
Milos  miotl, J ltapttt(k,1)*£5t5,k)>; 
end; 
end; 
end; 
for j:= 1 to nx5 do 
begin 


mlstnx5tl,jJ):= sq5*q5(j); 
end; 
nxSnu := nx5+nu; 
( writeln('enter hholder second time'); ) 
hholder (nx5nu,nx5,m15); 
for i:= 1 to nx5 do 


begin 
for™)y:= 1 townx5s do 
begin 

Speoisa,ii:= mi5li,J3); 

end; 

end; 

end; 
begin {*kk**kkbeginning of estimater*t****] 


{Writeln('time=',time, 'initial=',initial);) 
1f initial = 1 
then 
begin 
initializeArrays; 
Goro. 3; 
end; 
ud:=uf? 
fibttemiupput, xuwpic , nfo; 
yd := yf; 
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{ 


{ 


{ write('phi and yf 


filter(yoq pvt, xseeeec; net); 

zadd :=" Zar 

ZO) ii be 

Filter(z,zt,xz, Femme); 
update (phi,ud,yd,ns); 

phi (nx) :.=280-- 

nsnt := onset nt: 
time:=timetl; 

Lf time <= nsnf then goto 3; 


44% %*% 65.0 Wie em brdcme ~*~ I 


Rgain(nx, spe, t,o, pnljsmosa-hnat): 


newest(nx,xhat,khat,phi,y£); 
{ writeln('paras=', 
xhat(1),xhat(2),xhat(3),xhat(4),xhat(5)) ;) 


{*** compute blas estimate ***] 


1:= ns; 

while 1 >= 2 do 

begin 
beta(iJ:= betali - 1); 
hee: 1 ee 


end; 

beta(1l):= bias; 

bias:= xhat(nx}); 
Writeln('blas=',bias);) 


for j:= 1 to ns do 


[RRAKKKKKE 


begin 
bias:= bias + xhat[j]*betal[j)J; 
end; 
al:=xhatli);a2:=xhat(2);a3:=xhat[3);]) 


estimate xk exe 

phi2t(i} s= yf; 

Ohl21{ 27a. — 31.0; 

zZ£2 := (z£ - 2.0%zd + zdd)*sqr(£fs}; 

NZ» =: 
kgaln(n2,spt2,£,92,phi2,sr2,sq2,khat2); 


newest (n2,xhat2,khat2,ph12,2z£2); 
lf abs(xhat2(1)) > 0.00000001 
then gblas:= xhat2(2)/xhat2(1); 
vhat:= xhat2(1)}; 


*kk estimate pitch angle *** 


£311,1):= 1.0; 
£3{1,2}:= -1/£s; 
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Into newest', phill),phil2), piit3),ve£)}>) 
{ write('khat',khat(1),khat(2),khat(3)); 


} 


Elec ge) 
met? 2): 
aati]: 
gsot2): 
hott |: 
now 2a “Ox 

Kgain(nz,Spce,@e,g2,ne,sr3,sq3,khat3); ) 


0. 
1 
26 
0 
0 


* 
, 
s 
e 
a 
a 


Omer tl if 


1oeu Wm 


{observer for piytch ajh,10/2/88) 


begin 

bom: =leto 3 do 
begin 
xnew[ijJ:=0.0; 


fore J °:=1 to 3 .do 
xnewliJ:s=xnewlLI+£301,j)%*xs(J)l+tg3(il)*up/10.0+khat3(1)*(z-xs(3)]tyq-xs tl 


end; 

end;pitch:=xnewl[ 2); 

wee) s=xnew(1); 

xsi 2]:=xnew[(2]: 
xs([3):=xnew[3); 
al:=xs(1l);a2:=xs(2);a3:=xs(3); 


3:end; 


procedure GenerateDiveplaneCommand (var auvdepth,auvpitch,auvpitchrate, 
auvdepthcom,delta: realj;var k:integer ); 


{ This procedure takes digitized voltage values of depth,pitch,pitchrate, 
and target depth in volys and sends new commands for control. } 


const 
{ DT 2801-A DIGITAL TO ANALOG Conversion declarations ) 
d2achannelO = Q; 
pfs = 10.0; 
mfs = - 10.0; 
noc = 4096; 
T=0.2;amp=0.05;TO0=2.0; {these parameters include sin excitation) 


{ gl=-0.3066;92=0.8604;93=-0.2493; ) 
{these gains are for 20 Hz.@ poles=[(.92 .925 .926]] 
{ gl=-0.2286;g2=0.1944;g3=-0.0118; 
these gains are for 5Hz. and poles & [0.92 .925 .926)] 
Suee2060;,32-0,6573;93=-0.1605;{(for5Hz. at (0..85 0.860 0.870)]) 


var 
digitaldatavalueOd, status : integer; 
el,e2,e3 : real; 


Function ConvertAnalog2Digital ( analogvalue : real ): inteser; 


7a 


{ This function converts analog signal volts to an equivalent 


digital value. See App D of PCLAB book. ) 
var 
temp ¢o-F eas; 
begin 
temp := ( analogvalue - m—fs ) * ( (noc - 10) / (pfs - mfs ) ); 
convertanalog2digital := round ( temp ); 
end; 
begin ( 6332 aes GenerateDiveP laneComnand «= ----—---—.._-----___-— 


e3:=(auvdepthcom-auvdepth); 
e2:=(auvpitch); 
el:=(auvpitchrate); 


Bi abs(e3)<= 0.000001 then e3:=0.0; 
if abs(el)<=0.000005 then e1:=0.0; 
if abs(e2) <=0.000005 then e2:=0.0; 


delta s=g3%*e3 + gl*(-el) +g2%(-e2); 
if(abs(delta)>0.1) then 
begin 
delta :=0.1%*abs(delta)/delta; 
end; 
delta := 10*delta; 
digitaldatavalueO := convertanalog2digital ( delta i 
status := dacvalue ( d2achannelO, digitaldatavalueDd ); 
{ status := terminate; ) 
end; ( -“=2Se = >> oe GenerateDiveP laneCommand @.- —- ---==-----===—a 


procedure Movel(var r0,u,r :real); 
const 

countmax=400; 

count2=200; 

{type 

activecontrolmode = (run,reset,exit); 
auvattitude = (climb,maintain,diving); ) 


var 
count :integer i 
Gr, xlex2 x3 sreal ; 
activemode :activeCONTROLmode; 
attitude :auvattitude; 


begin 
activemode:= reset; 
attitude:=climb; 
count:=0 ; 
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while(count<countmax) do 
begin 
agms=0.010; 
GetDigitalSensoryData(x1l,x2,x3); 
if(count>count2)then dr:=0.0; 
re -rOtdr* count; 
(LE(count>count2)then dr:=0.0;) 
GenerateDivepJaneCommand (x1,x2,x3,r,u,count),; 
count:=counttl; 
{ UpdateRunModeScreen(x3,x2,xl,r,activemode,attitude) } 
end; 
end; 


procedure InitializeParameters ; 


{ This procedure initializes all declared control and display parameters to 
zero. 


begin | { ---- procedure InitializeParameters --- 
auvpitchvolts :=0.0 
auvdepthvolts := 
auvspeedvolts := 
auvpitchratevolts : 
auvdepth := 0.0; 
auvspeed := 0.0; 
auvpitchrate := 
estdepth:=0.0; 
err: =0.0; 

end; { ---- procedure InitializeParameters --- 


begin Maat: tumeess soso Ree VeContmol  —-——————=—— = ——----- 


initializeparameters; 

initial := 1; {initializeArrays in EST procedure} 
> 

time := 1; {initializes beginning of data file} 
Glrser ; 

writeln('DATA FILE NAME? [fe £082201.dat]'); 
readin(£ilename); 

assign(filevar, filename); 

rewrite(filevar); 

clrser: 

activemode := run; 


repeat { -------------- Repeat until activemode = exit ------ } 


{ ClosedLoopControlScreen; ) 
GetTargetDepth ( targetdepth, targetdepthvolts ); 
RunModeScreen; 


fi, 


dis 
while ( not keyhit ( modereply, modereply2) ) do 
begin 
updatecounter := 0; 


whtle ( updatecounter < updateincrement ) do 
begin 
GetDigitalSensoryData (auvdepthvolts,auvpitchvoltsxx,auvpitchratevolts); 


GenerateDilveplaneCommand ( ac3,ac2,acl, 
targeldepthvolts,divevolts,time ); 
EST (divevolts,auvp!itchratevolts,auvdepthvolts, 
speed,auvpitchvolts,initial,biasl,acl,ac2,ac3); 
writeln(filevar,time:5,auvdepthvolts:12:6,auvpitchvoltsxx:12:6, 
auvpitchratevolts:12:6,auvpitchvolts:8:3,divevolts:12:6, 
biasl:12m6, ae: 12:6,ac2:12:6,ac3:1276)- 


ATTITUDE_ (targetdepthvolts,auvdepthvolts,attitude) ; 
| initial :==0% 
updatecounter := updatecounter + 1; 
time := time + 1; 
end; { while updatecounter < updateincrement  ) 


UpdateRunMNodeScreen ( auvdepthvolts, auvpitchvoltsxx, 
auvpitchratevolts,divevolts,targetdepth, activemode, attitude,biasl1); 


end; { while not KeyHit ) 

1£ (ord(modereply) = 27) and (ord(modereply2) = 59) then { KeyHit=F1 ) 
begin 

Close(filevar); 

Joco. >. 

end 

else if (ord(modereply) = 27) and (ord(modereply2) = 60) then 


begin 


activemode := reset; 
Movel(targetdepthvolts,divevolts,tgtnew) ; 
targetdepthvolts:=tgtnew ; 

GOTO 1 

end 


else if (ord(modereply) = 27) and (ord(modereply2) =61) then 
begin 
close(filevar); 
activemode := exit; 
end { KeyHit= £3) 
until (activemode=exit) 


end; { SsS8--SS S552 s5S=—2>s aes ActiveControl ---==>-<==-<==] ue } 
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begin = ees 


repeat 


StatusAndCommandScreen; 
GetKey ('',mode,mode2); 


if { ord {mode) = 
begin 


27 ) and ( ord {mode2) = 59 ) then 


Elm ok.) x 1 py due Pees 


ClosedLoopControl; 


end; 


imtrr ( mode in ('Q',' 


end; Vee =~ 


> (ee) AN is 


procedure InitializeZeroDigitalSignalOut; 


{ This procedure MUST be 


executed as the first procedure called in the main 


program to insure a zero signal out on the 2 output channels. Otherwise the 
DT 2801-A board defaults to a minimum full scale output. } 
const 
digitalchan0O = 0; 
digitalchanl = 1; 
digitalcommandboard = 1; 
var 
status, 
digitaldatavalue : integer; 
begin 
digitaldatavalue := 2048; { This will be converted to an equivalent 
zero analog signal out ona 12 bit 
resolution converter like DT 2801-A. } 
status := initialize; 
status := selectboard ( digitalcommandboard ); 
status := dacvalue ( digitalchanO, digitaldatavalue ); 
status := dacvalue ( digitalchanl, digitaldatavalue ); 
status := terminate; 
end; 


procedure DeactivateADBoardAndExitProgram; 


{ This procedure deactivates the DT 26801-A board and presents an exit screen.} 


var 


fas, 


status : integer; 


begin 4 =<===-==32 2 oo] DeactivateADBoardAndExitProgram —-—-—--——--—-——_— ae } 
status := terminate; 
clr box2-( x1. yl. 7x2, v2.0; 
boxprint (y1l+10,x1l,x2,'THIS CONCLUDES YOUR AUV AUTOPILOTTING SESSION , BYE'); 


end; I DeactivateADBoardAndExitProgram ------------=-=-== } 
BEGIN a SS ee MAIN PROGRAM  $ ------—-3ae } 
InitializeZeroDigitalSignalout; 
Clrscer- 
repeat 


MainMenu ( option ); 


if ( option in i" R {22 J) eenen 


begin 
repeat 
begin 
StatusAndCommand ( controlmode ); 
end; 
until ( controlmode= mal a. 7 1. 
end; 


until ( option in 1 'O'4. a a): 
DeactivateADBoardAndExitProgram; 


END. { --------------------------------- MAIN PROGRAM _---------- 
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